import cv2
import numpy as np

# 读取并处理
img = cv2.imread("d:/F/Open Source/Prj_Robdog/xiaozhi-esp32-server/my_mcp/yolo8/ESP32_Slam/apartment.jpg")
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
_, binary = cv2.threshold(gray, 200, 255, cv2.THRESH_BINARY_INV)

# 适当膨胀一下让线条闭合
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
dilated = cv2.dilate(binary, kernel, iterations=1)

# 找轮廓，CCOMP方便后面找“外框”和“内部闭环”
contours, hierarchy = cv2.findContours(dilated, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)

# 寻找最大外框（面积最大的且没有父轮廓的那个）
areas = [cv2.contourArea(c) for c in contours]
max_idx = np.argmax(areas)

# 新建遮罩
mask = np.zeros(gray.shape, dtype=np.uint8)
# 先整张mask填充黑色（可导航区域）
# 只将最大外框外部填成黑，外框内部先填白，内部障碍后续再覆盖

# 先填最大外框为“可通行区域”
cv2.drawContours(mask, [contours[max_idx]], -1, 0, cv2.FILLED)  # 0=通行

# 遍历除最大外框以外的所有轮廓，全部标记为障碍物
for idx, cnt in enumerate(contours):
    # 过滤最大外框（外框的父索引hierarchy[0][idx][3] == -1）
    if idx != max_idx and hierarchy[0][idx][3] == max_idx:
        cv2.drawContours(mask, [cnt], -1, 255, cv2.FILLED)

# 还可以适当对mask做开运算去除小白点噪声
kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3,3))
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel2)

# 5. 保存结果到新文件
output_path = "d:/F/Open Source/Prj_Robdog/xiaozhi-esp32-server/my_mcp/yolo8/ESP32_Slam/mask.png"
cv2.imwrite(output_path, mask)
print(f"✅ 轮廓结果已保存至：{output_path}")

cv2.imshow("Obstacle Mask", mask)
cv2.waitKey(0)
cv2.destroyAllWindows()